/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_Studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#pragma once

#include <向量.h>
#include <几何数据.h>
//#include <矩阵.h>

#ifdef DEF_CUDA
	//#include <SIMD/GPU_向量计算.cuh>
#else
	//#include <SIMD/向量运算.h>
#endif


//#include <A_CUDA数学Core/aabbtree.h>
//#include <A_CUDA数学Core/voxelize.h>
//#include <A_CUDA数学Core/sdf.h>
//#include <B_几何图形/convex.h>

#include <assert.h>


//============================================= 平面 ======================================
Inline vec4 f_graph_plane(const vec3& n, float32 d) {
	return { n.x, n.y, n.z, d };
}

Inline float32 f_graph_点到平面距离(const vec3& point, const vec4& plane) {
	return plane.x * point.x + plane.y * point.y + plane.z * point.z + plane.w;
}

Inline vec4 f_graph_顶点构建平面(const vec3 point[3]) {
	vec3 e0 = point[1] - point[0];
	vec3 e1 = point[2] - point[0];
	vec3 n = vec_cross(e0, e1);

	float32 l = vec_dot(n, n);
	if (l > 0) {
		n *= f_逆平方根(l);
	}
	else {
		n = {};
	}

	return f_graph_plane(n, -vec_dot(point[0], n));
}

Inline vec4 f_graph_坐标构建平面(const vec3& point, const vec3& normal) {
	return f_graph_plane(normal, -f_graph_点到平面距离(point, f_graph_plane((normal), 0)));
}

Inline float32 f_graph_vectorAngle(const vec2& v) {
	return std::atan2(v.y, v.x);
}

vec3 f_graph_plane_WorldCoord(vec2 planarCoords, vec4 plane, vec3 upDirection);
vec2 f_graph_plane_PlaneCoord(vec3 position, vec4 plane, vec3 upDirection);




Inline bool f_graph_射线相交平面(const vec3& 起点, const vec3& 方向, const vec4& plane, float32& 相交宽度) {
	float32 dist = vec_dot(plane, 方向);

	if (dist > 0.0f && dist < 1.0f) {
		return false;
	}
	else {
		相交宽度 = -vec_dot(plane, 起点) / dist;
	}
	return true;
}

Inline bool f_graph_射线相交平面(const vec3& 起点, const vec3& 终点, const vec3& plane, vec3& out) {
	vec3 u = (终点 - 起点);

	float32 dist = -vec_dot(plane, 起点) / vec_dot(plane, u);

	if (dist > 0.0f && dist < 1.0f) {
		out = (起点 + u * dist);
		return true;
	}
	else return false;
}

Inline bool f_graph_线段平面相交(const vec3& 起点, const vec3& 终点, const vec4& plane, vec3& out) {
	vec3 u = (终点 - 起点);

	float32 dist = -vec_dot(plane, 起点 + vec3{ plane.x, plane.y, plane.z } * plane.w) / vec_dot(plane, u);

	if (dist > 0.0f && dist < 1.0f) {
		out = (起点 + u * dist);
		return true;
	}
	else return false;
}

Inline bool f_graph_线段平面相交(const vec3& 起点, const vec3& 终点, const vec4& plane) {
	vec3 u = (终点 - 起点);
	float32 dist = -vec_dot(plane, 起点 + vec3{ plane.x, plane.y, plane.z } * plane.w) / vec_dot(plane, u);

	if (dist > 0.0f && dist < 1.0f) {
		return true;
	}
	else return false;
}







typedef struct {
	vec3 pointA;
	vec3 pointB;
	vec3 pointC;
}S_三角形;

typedef struct {
	vec3	m_方向;
	vec3	m_端点;

	float32 m_长度;

	float32	m_封套S内半径;
	float32	m_封套E内半径;

	float32	m_封套S外半径;
	float32	m_封套E外半径;

} S_封套;





CUDA_CALLABLE inline float SampleSDF(const float* sdf, int dim, int x, int y, int z) {
	assert(x < dim && x >= 0);
	assert(y < dim && y >= 0);
	assert(z < dim && z >= 0);

	return sdf[z * dim * dim + y * dim + x];
}

CUDA_CALLABLE inline float f_SampleSDF(const float* sdf, const uvec3& dim, int32 x, int32 y, int32 z) {
	assert(uint32(x) < dim.x && x >= 0);
	assert(uint32(y) < dim.y && y >= 0);
	assert(uint32(z) < dim.z && z >= 0);

	return sdf[z * dim.x * dim.y + y * dim.x + x];
}

CUDA_CALLABLE Inline vec3 vec_Reflect(const vec3 vecI, const vec3 vecN) {
	//vecR = vecI - 2(vecI*vecN)vecN;
	//v' = v - 2 * (v*N) *N
	return vecI - (vecN * (vec_dot(vecI, vecN) * 2.0f));
}



template<typename T>
std::vector<T> f_Range(int64 s, int64 e) {
	int64 size = std::abs(e - s);
	std::vector<T> range(size);
	auto* p = range.data();
	for (uint32 i = 0; i < size; ++i) {
		p[i] = i;
	}
	return range;
}



CUDA_CALLABLE Inline vec3 vec_平方(const vec3& a) {
	return a * a;
};

CUDA_CALLABLE Inline float32 vec_和(const vec3& a) {
	return a.x + a.y + a.z;
};

CUDA_CALLABLE Inline float32 vec_平方范数(vec3 v) {
	v = vec_平方(v);
	return vec_和(v);
}




//vec3 f_graph_距离最近坐标(vec3 原点, vec3* 坐标集, uint32 num);
Inline vec3 f_graph_距离最近坐标(vec3 原点, vec3* 坐标集, uint32 num) {
	float32 dis = vec_len(原点 - 坐标集[0]);
	vec3 最近点 = 坐标集[0];
	for (uint32 i = 1; i < num; ++i) {
		float32 dis_temp = vec_len(原点 - 坐标集[i]);
		if (dis_temp < dis) {
			dis = dis_temp;
			最近点 = 坐标集[i];
		}
	}
	return 最近点;
}




//取三角形AB边
Inline vec3 geometry_getTriangleEdgeAB(const S_三角形& triangle) {
	vec3 vecAB;
	vecAB.x = triangle.pointB.x - triangle.pointA.x;
	vecAB.y = triangle.pointB.y - triangle.pointA.y;
	vecAB.z = triangle.pointB.z - triangle.pointA.z;
	return vecAB;
}

//取三角形BC边
Inline vec3 geometry_getTriangleEdgeBC(const S_三角形& triangle) {
	vec3 vecBC;
	vecBC.x = triangle.pointC.x - triangle.pointB.x;
	vecBC.y = triangle.pointC.y - triangle.pointB.y;
	vecBC.z = triangle.pointC.z - triangle.pointB.z;
	return vecBC;
}

//取三角形AC边
Inline vec3 geometry_getTriangleEdgeAC(const S_三角形& triangle) {
	vec3 vecAC;
	vecAC.x = triangle.pointC.x - triangle.pointA.x;
	vecAC.y = triangle.pointC.y - triangle.pointA.y;
	vecAC.z = triangle.pointC.z - triangle.pointA.z;
	return vecAC;
}


Inline float f_graph_点到三角形平面距离(const S_三角形& triangle, const vec3& OutSidePoint) {
	vec3 vecAB = geometry_getTriangleEdgeAB(triangle);
	vec3 vecAC = geometry_getTriangleEdgeAC(triangle);

	vec3 vecPointToPlane =
	{ OutSidePoint.x - triangle.pointA.x, OutSidePoint.y - triangle.pointA.y, OutSidePoint.z - triangle.pointA.z };

	vecAB = vec_normalize2(vecAB);
	vecAC = vec_normalize2(vecAC);
	//计算平面法线
	vec3 PNormal = vec_cross(vecAB, vecAC);
	float dis = vec_normalize(&PNormal) + DEF_浮点精度补充;
	
	//得到点和平面的距离
	return vec_dot(vecPointToPlane, PNormal) / dis;
}


Inline float64 f_graph_两向量夹角弧度(const vec3& vec0, const vec3& vec1) {
	float64 角A = abs(vec_dot(vec0, vec1));
	float64 角B = vec_len(vec0) * vec_len(vec1);
	if(角B == 0) 角B = DEF_浮点精度补充;
	//return acos(角A / 角B);
	return acos( DEF_Min(DEF_Max(角A / 角B, 0), 1) );
}

Inline float64 f_graph_vectorAngle(const vec3& vec0, const vec3& vec1) {
	float64 角A = abs(vec_dot(vec0, vec1));
	if (角A > 1.0f) 角A = 1.0f;
	if (角A < -1.0f) 角A = -1.0f;
	return acos( 角A );
}

float32 f_graph_两向量夹角弧度_export(const vec3& vec0, const vec3& vec1);

Inline float64 f_graph_两向量夹角弧度180(const vec3& vec0, const vec3& vec1) {
	float32 角A = vec_dot(vec0, vec1);
	float32 角B = vec_len(vec0) * vec_len(vec1);

	角B = acos(fabs(角A) / (角B + DEF_浮点精度补充));
	if (角A < 0) {
		角B = 3.14159265 - 角B;
	}
	return 角B;
}

Inline float64 f_graph_两向量夹角弧度(const vec2& vec0, const vec2& vec1) {
	float32 角A = (vec_dot(vec0, vec1));
	float32 角B = vec_len(vec0) * vec_len(vec1);
	return acos( DEF_Min(DEF_Max(角A / (角B + DEF_浮点精度补充), -1), 1) );
}
Inline float64 f_graph_两向量夹角弧度180(const vec2& vec0, const vec2& vec1) {
	float32 角A = (vec_dot(vec0, vec1));
	float32 角B = vec_len(vec0) * vec_len(vec1);
	角B = acos( DEF_Min(fabs(角A) / (角B + DEF_浮点精度补充), 1) );
	if (角A < 0) {
		角B = 3.14159265 - 角B;
	}
	return 角B;
}



Inline float64 f_graph_三角函数_临边计算斜边长度(const float64 临边B长度, const float64 角弧度) {
	return 临边B长度 / (cos(角弧度) + DEF_浮点精度补充);
}
Inline float64 f_graph_三角函数_斜边计算对边长度(const float64 斜边C长度, const float64 角弧度) {
	return sin(角弧度) * 斜边C长度;
}
Inline float64 f_graph_三角函数_斜边计算临边长度(const float64 斜边C长度, const float64 角弧度) {
	return cos(角弧度) * 斜边C长度;
}
Inline float64 f_graph_三角函数_对边计算斜边长度(const float64 对边A长度, const float64 角弧度) {
	return 对边A长度 / (sin(角弧度) + DEF_浮点精度补充);
}
Inline float64 f_graph_三角函数_对边计算临边长度(const float64 对边A长度, const float64 角弧度) {
	return 对边A长度 / (tan(角弧度) + DEF_浮点精度补充);
}
Inline float64 f_graph_三角函数_临边计算对边长度(const float64 对边A长度, const float64 角弧度) {
	return 对边A长度 * tan(角弧度);
}


float32 f_graph_两线段距离(const vec3& vec0, const vec3& vec1, const vec3& line起点, const vec3& line方向单位向量);
float32 f_graph_两线段距离(const vec3& vec0, const vec3& vec1, const vec3& line起点, const vec3& line方向单位向量, vec3& 相交点, float32 线段补偿距离 = 0);

Inline float32 f_graph_点到直线距离(const vec3& s, const vec3& dir, const vec3& point) {
	vec3 x = point - s;
	float32 l = vec_normalize(&x);
	return f_graph_三角函数_斜边计算对边长度(l, f_graph_两向量夹角弧度(x, dir));
}


Inline vec3 f_graph_计算点到直线垂足坐标(const vec3& s, const vec3& dir, const vec3& point) {
	vec3 x = point - s;
	float32 l = vec_normalize(&x);
	float32 临边长度 = f_graph_三角函数_斜边计算临边长度(l, f_graph_两向量夹角弧度(x, dir));
	return s + dir* 临边长度;
}

Inline float32 f_graph_点到线段的距离(const vec2& point, const vec2& lineP1, const vec2& lineP2, float32& 垂足) {

	vec2 a = point - lineP1;
	vec2 b = lineP2 - lineP1;
	垂足 = dot(a, b) / dot(b, b);

	if (垂足 < 0) {
		return length(point - lineP1);
	}
	if (垂足 > 1) {
		return length(point - lineP2);
	}
	return length(point - (lineP1 + (b * 垂足)));
}


Inline float32 f_graph_点到线段的距离(const vec2& point, const vec2& lineP1, const vec2& lineP2) {
	
	vec2 a = point - lineP1;
	vec2 b = lineP2 - lineP1;
	float32 投影 = dot(a,a) / dot(b,b);

	投影 = dot(a, b) / dot(b, b);

	if (投影 < 0) {
		return length(point - lineP1);
	}
	if (投影 > 1) {
		return length(point - lineP2);
	}
	return length(point - (lineP1 + (b * 投影)));
}

vec3 f_graph_从向量计算旋转角(const vec3& 向量1, const vec3& 向量2);






//======================================================= 四元数计算 ==========================================================

Inline vec4 f_graph_quat相乘(const vec4& q1, const vec4& q2) {
	vec4 newPos;
	newPos.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q2.y * q1.z;
	newPos.y = q1.w * q2.y + q2.w * q1.y + q1.z * q2.x - q2.z * q1.x;
	newPos.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q2.x * q1.y;
	newPos.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q2.z * q1.z;
	return newPos;
}

Inline vec4 f_graph_quat反向(const vec4& q) {
	return { -q.x, -q.y, -q.z, q.w };
}

Inline vec3 f_graph_quat变换坐标(const vec4& q, const vec3& pos) {
	vec3 newPos;
	vec3 qv = { q.x, q.y, q.z };
	newPos = pos * (2.0f * q.w * q.w - 1.0f);
	newPos = newPos - (vec_cross(qv, pos) * q.w * 2.0f) + (qv * vec_dot(vec3{ q.x, q.y, q.z }, pos) * 2.0f);
	return newPos;
}

CUDA_CALLABLE inline vec4 f_graph_欧拉角转四元数2(const vec3& axis) {
	vec3 xyz = axis;// *0.5f;
	//(xyz *= (xyz, float32(0.5)));
	xyz.x = M_角度转弧度(xyz.x);
	xyz.y = M_角度转弧度(xyz.y);
	xyz.z = M_角度转弧度(xyz.z);

	vec_mul(&xyz, 0.5);
	vec4 q;

	q.x = sin(xyz.y) * sin(xyz.z) * cos(xyz.x) + cos(xyz.y) * cos(xyz.z) * sin(xyz.x);
	q.y = sin(xyz.y) * cos(xyz.z) * cos(xyz.x) + cos(xyz.y) * sin(xyz.z) * sin(xyz.x);
	q.z = cos(xyz.y) * sin(xyz.z) * cos(xyz.x) - sin(xyz.y) * cos(xyz.z) * sin(xyz.x);
	q.w = cos(xyz.y) * cos(xyz.z) * cos(xyz.x) - sin(xyz.y) * sin(xyz.z) * sin(xyz.x);

	return q;
}

CUDA_CALLABLE inline vec4 f_graph_欧拉角转四元数(const vec3& axis) {
	vec3 xyz = axis;
	xyz.x = M_角度转弧度(xyz.x);
	xyz.y = M_角度转弧度(xyz.y);
	xyz.z = M_角度转弧度(xyz.z);

	//float32 TEST = q.w * q.y - q.x * q.z;
	vec_mul(&xyz, 0.5);
	vec4 q;

	float32 cy = cos(xyz.z);
	float32 sy = sin(xyz.z);
	float32 cp = cos(xyz.y);
	float32 sp = sin(xyz.y);
	float32 cr = cos(xyz.x);
	float32 sr = sin(xyz.x);

	q.w = cr * cp * cy + sr * sp * sy;
	q.x = sr * cp * cy - cr * sp * sy;
	q.y = cr * sp * cy + sr * cp * sy;
	q.z = cr * cp * sy - sr * sp * cy;

	return q;
}


CUDA_CALLABLE Inline vec3 f_graph_四元数转欧拉角(const vec4& q) {
	vec3 angles;

	/*double q0, q1, q2, q3;
	q0 = q.w;
	q1 = q.x;
	q2 = q.y;
	q3 = q.z;
	angles.y = (float)(asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3); // pitch
	angles.x = (float)(atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3); // roll
	angles.z = (float)(atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3);

	//angles.x = M_弧度转角度(angles.x);
	//angles.y = M_弧度转角度(angles.y);
	//angles.z = M_弧度转角度(angles.z);

	return angles;*/
	/*float32 r11 = (-2 * (q.y * q.z - q.w * q.x));
	float32 r12 = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
	float32 r21 = 2 * (q.x * q.z + q.w * q.y);
	float32 r31 = -2 * (q.x * q.y - q.w * q.z);
	float32 r32 = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;

	angles.x = atan2(r31, r32);
	angles.y = asin(r21);
	angles.z = atan2(r11, r12);

	angles.x = M_弧度转角度(angles.x);
	angles.y = M_弧度转角度(angles.y);
	angles.z = M_弧度转角度(angles.z);

	return angles;*/

	// roll (x-axis rotation)
	float64 sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
	float64 cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
	angles.x = atan2(sinr_cosp, cosr_cosp);

	// pitch (y-axis rotation)
	double sinp = 2 * (q.w * q.y - q.z * q.x);
	if (abs(sinp) >= 1)
		angles.y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		angles.y = asin(sinp);

	// yaw (z-axis rotation)
	float64 siny_cosp = 2 * (q.w * q.z + q.x * q.y);
	float64 cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
	angles.z = atan2(siny_cosp, cosy_cosp);

	angles.x = M_弧度转角度(angles.x);
	angles.y = M_弧度转角度(angles.y);
	angles.z = M_弧度转角度(angles.z);

	return angles;
}



//inline Quat f_Quat_转换(const vec3& rhs) {
//	return Quat(rhs.x, rhs.y, rhs.z, 0.0f);
//}

//inline Quat f_Quat_转换(const vec4& rhs) {
//	return Quat(rhs.x, rhs.y, rhs.z, rhs.w);
//}

CUDA_CALLABLE inline vec4 f_graph_quatNormalize(const vec4& q) {
	float32 lSq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;

	if (lSq > 0.0f) {
		float32 invL = 1.0f / sqrtf(lSq);
		return q * invL;
	}
	else
		return { 0,0,0,1 };
}

Inline vec4 f_graph_从轴向角度计算四元数(const vec3& 归一化向量, float 弧度) {
	float32 half = 弧度 * 0.5f;
	float32 w = cosf(half);

	const float32 sin_theta_over_two = sinf(half);
	vec3 v = 归一化向量 * sin_theta_over_two;

	return f_graph_quatNormalize({ v.x, v.y, v.z, w });
}

Inline vec4 f_graph_旋转角转四元数(const vec3& 旋转角) {
	vec4 x = f_graph_从轴向角度计算四元数({ 1,0,0 }, M_角度转弧度(旋转角.x));
	vec4 y = f_graph_从轴向角度计算四元数({ 0,0,1 }, M_角度转弧度(旋转角.y));
	vec4 z = f_graph_从轴向角度计算四元数({ 0,1,0 }, M_角度转弧度(旋转角.z));
	x = f_graph_quat相乘(x, z);
	x = f_graph_quat相乘(x, y);
	//x = f_graph_quat相乘(y, x);
	//x = f_graph_quat相乘(z, x);
	return x;
}

Inline vec4 f_graph_convertRotation(const vec3& rot) {
	float32 halfRoll = rot.x * 0.5;
	float32 halfPitch = rot.y * 0.5;
	float32 halfYaw = rot.z * 0.5;

	float32 sinRoll = sin(halfRoll);
	float32 cosRoll = cos(halfRoll);
	float32 sinPitch = sin(halfPitch);
	float32 cosPitch = cos(halfPitch);
	float32 sinYaw = sin(halfYaw);
	float32 cosYaw = cos(halfYaw);

	vec4 q;
	q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
	q.x = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
	q.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
	q.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

	return q;
}

Inline vec4 f_graph_convertRotationToQuat(const vec3& rot) {
	vec4 q;

	// 将旋转角度转换为弧度
	float32 radX = rot.x * M_PI / 180.0;
	float32 radY = rot.y * M_PI / 180.0;
	float32 radZ = rot.z * M_PI / 180.0;

	// 计算旋转角度的一半
	float32 halfAngleX = radX / 2.0;
	float32 halfAngleY = radY / 2.0;
	float32 halfAngleZ = radZ / 2.0;

	// 计算旋转轴的sin和cos值
	float32 sinX = sin(halfAngleX);
	float32 cosX = cos(halfAngleX);

	float32 sinY = sin(halfAngleY);
	float32 cosY = cos(halfAngleY);

	float32 sinZ = sin(halfAngleZ);
	float32 cosZ = cos(halfAngleZ);

	// 计算四元数的分量
	q.w = cosX * cosY * cosZ + sinX * sinY * sinZ;
	q.x = sinX * cosY * cosZ - cosX * sinY * sinZ;
	q.y = cosX * sinY * cosZ + sinX * cosY * sinZ;
	q.z = cosX * cosY * sinZ - sinX * sinY * cosZ;

	return q;
}

Inline vec3 f_graph_quatTranform(const vec4& q, const vec3& pos) {
	vec3 qv = { q.x, q.y, q.z };
	vec3 newPos = pos * (2.0f * q.w * q.w - 1.0f);
	newPos = newPos + (vec_cross(qv, pos) * q.w * 2.0f) + (qv * vec_dot(qv, pos) * 2.0f);
	return newPos;
}

CUDA_CALLABLE Inline vec3 vec_绕轴旋转(const vec4& dir, const vec3 oldPos) {
	vec3 r;
	//float32 a = dir.w * M_PI / 180;
	float32 a = dir.w;
	float32 c = cos(a);
	float32 s = sin(a);

	r.x = (dir.x * dir.x * (1 - c) + c) * oldPos.x + (dir.x * dir.y * (1 - c) - dir.z * s) * oldPos.y + (dir.x * dir.z * (1 - c) + dir.y * s) * oldPos.z;
	r.y = (dir.y * dir.x * (1 - c) + dir.z * s) * oldPos.x + (dir.y * dir.y * (1 - c) + c) * oldPos.y + (dir.y * dir.z * (1 - c) - dir.x * s) * oldPos.z;
	r.z = (dir.x * dir.z * (1 - c) - dir.y * s) * oldPos.x + (dir.y * dir.z * (1 - c) + dir.x * s) * oldPos.y + (dir.z * dir.z * (1 - c) + c) * oldPos.z;

	return r;
}

Inline vec4 f_graph_quat两向量(const vec3& 归一化_dirA, const vec3& 归一化_dirB) {
	float32 弧度AB = f_graph_两向量夹角弧度180(归一化_dirA, 归一化_dirB);
	return f_graph_从轴向角度计算四元数(vec_normalize2(vec_cross(归一化_dirA, 归一化_dirB)), 弧度AB);
}

#define IX(x, y, z) ((x) + (y) * N + (z) * N * N)
CUDA_CALLABLE Inline uint64 f_graph_坐标转线性索引(ivec3 coord, const ivec3& dim) {
	coord = clamp(coord, ivec3{0,0,0}, dim-1);
	uint32 p = dim.x * dim.y;
	return uint64(coord.z) * p + uint64(coord.y) * dim.x + coord.x;
}

CUDA_CALLABLE Inline uvec3 f_graph_线性索引转坐标(const uint64 index, const uvec3& dim) {
	uint32 p = dim.x * dim.y;
	uvec3 coord;
	coord.z = index / p;
	coord.y = (index - (coord.z * p)) / dim.x;
	coord.x = index - ((coord.z * p) + (coord.y * dim.x));
	return coord;
}

CUDA_CALLABLE Inline uint64 f_坐标转线性索引_i(const ivec3& coord, const uvec3& dim) {
	uint32 p = dim.x * dim.y;
	return uint64(coord.z) * p + uint64(coord.y) * dim.x + coord.x;
}

CUDA_CALLABLE Inline uvec3 f_浮点坐标转索引坐标(const vec3& coord, const vec3& offset, float32 间隔缩放 = 1.0) {
	uvec3 dim;
	dim.x = (coord.x - offset.x) / 间隔缩放;
	dim.y = (coord.y - offset.y) / 间隔缩放;
	dim.z = (coord.z - offset.z) / 间隔缩放;
	return dim;
}

CUDA_CALLABLE Inline vec3 f_矩阵相乘(const mat3X3& m1, const vec3& v2) {
	vec3 r = vec_mul(m1.row0, v2.x);
	vec_add(&r, vec_mul(m1.row1, v2.y));
	vec_add(&r, vec_mul(m1.row2, v2.z));
	return r;
}

CUDA_CALLABLE Inline mat3X3 f_矩阵相乘(const mat3X3& m1, const mat3X3& m2) {
	mat3X3 r;
	r.row0 = f_矩阵相乘(m1, m2.row0);
	r.row1 = f_矩阵相乘(m1, m2.row1);
	r.row2 = f_矩阵相乘(m1, m2.row2);
	return r;
}

//CUDA_CALLABLE Inline mat3X3 f_矩阵相乘(const mat3X3& m1, const Matrix33& m2) {
//	mat3X3 r;
//	r.row0 = f_矩阵相乘(m1, *((vec3*)&m2.cols[0]));
//	r.row1 = f_矩阵相乘(m1, *((vec3*)&m2.cols[1]));
//	r.row2 = f_矩阵相乘(m1, *((vec3*)&m2.cols[2]));
//	return r;
//}

CUDA_CALLABLE std::vector<vec3> vec_Mul(const std::vector<vec3>& sv, const float32 s);
CUDA_CALLABLE std::vector<vec3> vec_Mul(const std::vector<vec4>& sv, const float32 s);







//======================================================= 相交计数 ==========================================================

//射线三角形相交
bool IntersectTriangle(const float* vert1, const float* vert2, const float* vert3, const float* near, const float* far, float* uvt);

//射线正面相交三角形
bool FrontIntersectTriangle(const float* vert1, const float* vert2, const float* vert3,
	const float* near, const float* far, float* uvt, bool isFrant = true);

//int32 f_IntersectTriangle(const vec3& vert1, const vec3& vert2, const vec3& vert3, const vec3& N, const vec3& dir, vec3* uvt);
int32 f_graph_IntersectTriangle(const vec3& vert1, const vec3& vert2, const vec3& vert3, const vec3& S, const vec3& dir, vec3* uvt);




//根据UVT取得焦点位置
inline void Intersect_uvtPos(const float vert1[3], const float vert2[3], const float vert3[3], const float uvt[3], float point[3]) {
	float vecAB[3] = { 0.0,0.0,0.0 };
	float vecAC[3] = { 0.0,0.0,0.0 };
	vec_相减(vecAB, vert2, vert1);
	vec_相减(vecAC, vert3, vert1);

	vec_自乘(vecAB, uvt[0]);
	vec_自乘(vecAC, uvt[1]);
	vec_相加(point, vert1, vecAB);
	vec_相加(point, vecAC, point);
}

//是否同侧
inline bool f_点在边同侧(const float* A, const float* B, const float* C, const float* P) {
	float AB[3] = { B[0] - A[0], B[1] - A[1], B[2] - A[2] };
	float AC[3] = { C[0] - A[0], C[1] - A[1], C[2] - A[2] };
	float AP[3] = { P[0] - A[0], P[1] - A[1], P[2] - A[2] };
	
	float v1[3] = { 0.0, 0.0, 0.0 };
	vec_叉积(v1, AB, AC);
	float v2[3] = { 0.0, 0.0, 0.0 };
	vec_叉积(v2, AB, AP);

	return vec_dot(v1, v2) >= 0;
}

inline bool f_点在边同侧(const vec3& A, const vec3& B, const vec3& C, const vec3& P) {
	vec3 AB = B - A;
	vec3 AC = C - A;
	vec3 AP = P - A;
	
	vec3 v1{ 0.0, 0.0, 0.0 };
	v1 = vec_cross(AB, AC);
	vec3 v2 = { 0.0, 0.0, 0.0 };
	v2 = vec_cross(AB, AP);

	return vec_dot(v1, v2) >= 0;
}

inline bool f_点是否在三角形内(const float* A, const float* B, const float* C, const float* P) {
	return f_点在边同侧(A, B, C, P) && f_点在边同侧(B, C, A, P) && f_点在边同侧(C, A, B, P);
}

bool PointinTriangle(const float* A, const float* B, const float* C, const float* P);

typedef struct {
	vec3*	vert;
	uint32*	index;
	uint32	num;
	vec3	normal;
}S_PolygonFace;
bool f_graph_FaceIntersect(const S_PolygonFace& faceA, const S_PolygonFace faceB);







//两线段距离
Inline float f_graph_两线段距离(const float* vec0, const float* vec1) {
	float normal[3] = { 0.0, 0.0, 0.0 };
	vec_叉积(normal, vec0, vec1);
	float dot = vec_dot(vec0, vec1);
	return dot / 向量_两向量距离(normal);
}

int32	f_UpProject还原空间顶点(const vec3* xyz, const float32* 投影矩阵, const float32* 模型矩阵, const vec4* viewport, vec4* vert);
vec3	f_UpProject还原空间顶点(const vec3& xyz, const Mat44f& 投影矩阵, const Mat44f& 视图逆矩阵, const S_Rect2Df& viewport);
vec2	f_Project空间坐标转屏幕(const Mat44f& 投影矩阵, const Mat44f& 视图矩阵, const vec3& 坐标, const S_Rect2Df& rect);

vec3	f_proj_空间坐标转屏幕(const Mat44f& 投影矩阵, const Mat44f& 视图矩阵, const vec3& 坐标, const S_Rect2Df& rect);

//bool	f_graph_AABB射线相交(const vec3& start, const vec3& dir, const vec3& min, const vec3& max, float32& t);

void f_Voxelize遮罩(const vec3* 顶点, int 顶点num, const uint32* 索引, int 索引数量, uint32 width, uint32 height, uint32 depth, uint8* volume, vec3 minExtents, vec3 maxExtents);


bool f_graph_点是否在矩形内(const S_Rect2Df& rect, vec2* v, uint32 num);

int32 f_graph_线段相交(const S_Line& 主线, const vec2* lines, uint32 num);
int32 f_graph_线段相交(const S_Line& 主线, const vec2* lines, uint32 num, vec2* 相交坐标);

bool f_graph_射线与变换AABB相交(const S_Bounding& box, const vec3& s, const vec3& dir, vec3* 相交坐标, float32* 相交宽度);

//抄了NV的 FleX IntersectRayAABBOmpf
bool f_graph_射线AABB相交(const vec3& s, const vec3& dir, const S_Bounding& box, float32* t = nullptr);

float32 f_graph_点与胶囊相交(const vec3& 点, const vec3& A端点坐标, const vec3& 方向, float32 高度, float32 半径);




// 反射光线
/*void reflectRay(inout vec3 rayD, in vec3 mormal)
{
	rayD = rayD + 2.0 * -dot(mormal, rayD) * mormal;
}

// 光线与圆球求交
float sphereIntersect(in vec3 rayO, in vec3 rayD, in Sphere sphere)
{
	vec3 oc = rayO - sphere.pos;
	float b = 2.0 * dot(oc, rayD);
	float c = dot(oc, oc) - sphere.radius * sphere.radius;
	float h = b * b - 4.0 * c;
	if (h < 0.0)
	{
		return -1.0;
	}
	float t = (-b - sqrt(h)) / 2.0;

	return t;
}

vec3 sphereNormal(in vec3 pos, in Sphere sphere)
{
	return (pos - sphere.pos) / sphere.radius;
}

// 光线与平面求交
float planeIntersect(vec3 rayO, vec3 rayD, Plane plane)
{
	float d = dot(rayD, plane.normal);

	if (d == 0.0)
		return 0.0;

	float t = -(plane.distance + dot(rayO, plane.normal)) / d;

	if (t < 0.0)
		return 0.0;

	return t;
}*/


/*Inline float f_点到平面距离(const S_三角形& triangle, const float OutSidePoint[3], float PNormal[3]) {
	float vecAB[3] = { 0.0,0.0,0.0 };
	float vecAC[3] = { 0.0,0.0,0.0 };
	geometry_getTriangleEdgeAB(triangle, vecAB);
	geometry_getTriangleEdgeAC(triangle, vecAC);
	float vecPointToPlane[3] =
	{ OutSidePoint[0] - triangle.pointA.x, OutSidePoint[1] - triangle.pointA.y, OutSidePoint[2] - triangle.pointA.z };

	vec_归一化(vecAB);
	vec_归一化(vecAC);
	//计算平面法线
	vec_叉积(vecAB, vecAC, PNormal);
	float dis = vec_归一化(PNormal);

	//得到点和平面的距离
	return vec_dot(vecPointToPlane, PNormal) / dis;
}*/

//Inline vec3 f_graph_从向量计算欧拉角(const vec3& dir) {
//	vec2 zdir = { dir.x, dir.y };
//	float32 z = f_graph_两向量夹角弧度180(zdir, { 1,0 });
//}




